/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2024-03-04 18:43:24
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-03-14 16:46:40
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\factorytest\factorytest.c
 * @Description: 出厂测试程序，测试完成后请记得注释掉这一段代码。
 * 测试方法：1.开机后如果为白色快闪，则代表某个传感器硬件有问题，需要插上串口查看打印信息。
 *          2.如果是紫色灯光慢闪进入测试，拿起飞控快速的前后左右转动，灯光由紫色慢闪变为蓝色呼吸灯说明出厂测试通过，如果多次转动后还是紫色慢闪说明飞控有问题。
 */
#include "fc_error.h"
#include "fc_sensors.h"
#include "zino.h"
#include "usercmd.h"
#include "icm42688p_read_polling.h"
#include <rtthread.h>
#define DBG_TAG "factorytest"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define OPT_DEADBAND_VALUE 5
#define ACC_GYRO_DEADBAND_VALUE 500
uint8_t check_sensor_direction = 0;

ZINO_CONFIG_DECLARE(uint8_t, FatSave);
ZINO_CONFIG_REGISTER(uint8_t, FatSave, 3, 1);

enum
{
    // ACC_UP = 1 << 0,
    // ACC_DOWN = 1 << 1,
    // ACC_LEFT,
    // ACC_RIGHT,
    // ACC_FORWARD,
    // ACC_BACKWARD,
    OPT_GYRO_ACC_LEFT = 1 << 0,
    OPT_GYRO_ACC_RIGHT = 1 << 1,
    OPT_GYRO_ACC_FORWARD = 1 << 2,
    OPT_GYRO_ACC_BACKWARD = 1 << 3,
    LASER_ALT = 1 << 4,
    BARO_ALT = 1 << 5,
    RC_GET = 1 << 6,
};

// typedef struct
// {
//     int16_t acc[3];
//     int16_t gyro[3];
// }FT_ACC_GYRO_S;

// FT_ACC_GYRO_S ft_icm_data;

void factorytest_handle()
{
    static uint8_t sensor_test_mode = 0;
    int16_t icm_acc[3], icm_gyro[3], pmw_opt[3];
    static int16_t ICM_ACC[3], ICM_GYRO[3], PWM_OPT[3];
    static int16_t laseralt, baroalt;
    if (*FatSave() == 0x7f)
        return;
    if (FC_ERROR_CHECK(FC_ACC_GYRO_HARDFAULT) || FC_ERROR_CHECK(FC_LASER_HARDFAULT) || FC_ERROR_CHECK(FC_BARO_HARDFAULT) || FC_ERROR_CHECK(FC_OPTICALFLOW_HARDFAULT))
    {
        FC_ERROR_RESET(FC_FACTORY_TEST);
        return;
    }
    if (sensor_test_mode == 0)
    {
        if (!FC_ERROR_CHECK(FC_FACTORY_TEST))

            FC_ERROR_SET(FC_FACTORY_TEST);

        icm_acc[0] = fc_sensor_data.acc.accdata.x * 4096;
        icm_acc[1] = fc_sensor_data.acc.accdata.y * 4096;
        icm_acc[2] = fc_sensor_data.acc.accdata.z * 4096;

        icm_gyro[0] = fc_sensor_data.gyro.gyrodata.x * 16;
        icm_gyro[1] = fc_sensor_data.gyro.gyrodata.y * 16;
        icm_gyro[2] = fc_sensor_data.gyro.gyrodata.z * 16;
        pmw_opt[0] = fc_sensor_data.optf_raw.optVelX;
        pmw_opt[1] = fc_sensor_data.optf_raw.optVelY;

        laseralt += (fc_sensor_data.tof.range - laseralt) / 2;
        baroalt += (fc_sensor_data.baro.altitude - baroalt) / 2;
        // baroalt = fc_sensor_data.baro.altitude;

        for (uint8_t i = 0; i < 3; i++)
        {

            ICM_ACC[i] += (icm_acc[i] - ICM_ACC[i]) / 5;
            ICM_GYRO[i] += (icm_gyro[i] - ICM_GYRO[i]) / 5;
            PWM_OPT[i] += (pmw_opt[i] - PWM_OPT[i]) / 2;
        }

        if (ICM_GYRO[0] < -ACC_GYRO_DEADBAND_VALUE && ICM_ACC[1] < -ACC_GYRO_DEADBAND_VALUE && PWM_OPT[0] < -OPT_DEADBAND_VALUE)
        {
            check_sensor_direction |= OPT_GYRO_ACC_LEFT;
            // LOG_D("ICM_ACC[0] = %d,ICM_GYRO[1] = %d,PWM_OPT[0] = %d", ICM_ACC[0], ICM_GYRO[1], PWM_OPT[1]);
            LOG_D("OPT_GYRO_ACC_LEFT\n");
        }
        if (ICM_GYRO[0] > ACC_GYRO_DEADBAND_VALUE && ICM_ACC[1] > ACC_GYRO_DEADBAND_VALUE && PWM_OPT[0] > OPT_DEADBAND_VALUE)
        {
            check_sensor_direction |= OPT_GYRO_ACC_RIGHT;
            // LOG_D("ICM_ACC[0] = %d,ICM_GYRO[1] = %d,PWM_OPT[0] = %d", ICM_ACC[0], ICM_GYRO[1], PWM_OPT[1]);
            LOG_D("OPT_GYRO_ACC_RIGHT\n");
        }
        if (ICM_GYRO[1] > ACC_GYRO_DEADBAND_VALUE && ICM_ACC[0] < -ACC_GYRO_DEADBAND_VALUE && PWM_OPT[1] > OPT_DEADBAND_VALUE)
        {
            check_sensor_direction |= OPT_GYRO_ACC_FORWARD;
            // LOG_D("ICM_ACC[0] = %d,ICM_GYRO[1] = %d,PWM_OPT[1] = %d", ICM_ACC[0], ICM_GYRO[1], PWM_OPT[1]);
            LOG_D("OPT_GYRO_ACC_FORWARD\n");
        }
        if (ICM_GYRO[1] < -ACC_GYRO_DEADBAND_VALUE && ICM_ACC[0] > ACC_GYRO_DEADBAND_VALUE && PWM_OPT[1] < -OPT_DEADBAND_VALUE)
        {
            check_sensor_direction |= OPT_GYRO_ACC_BACKWARD;
            // LOG_D("ICM_ACC[0] = %d,ICM_GYRO[1] = %d,PWM_OPT[1] = %d", ICM_ACC[0], ICM_GYRO[1], PWM_OPT[1]);
            LOG_D("OPT_GYRO_ACC_BACKWARD\n");
        }
        if (laseralt)
        {
            check_sensor_direction |= LASER_ALT;
        }
        if (baroalt)
        {
            check_sensor_direction |= BARO_ALT;
        }
        if (usercmd.rc_rssi >= 2)
        {
            check_sensor_direction |= RC_GET;
        }
        if (check_sensor_direction == 0x7f)
        {
            FC_ERROR_RESET(FC_FACTORY_TEST);
            sensor_test_mode = 1;
            *FatSave() = check_sensor_direction;
            zino_config_save_all();
        }
    }
}

void factorytest_handle_task(void *P)
{
    while (1)
    {
        factorytest_handle();
        rt_thread_mdelay(50);
    }
}

void factorytest_handle_task_entry(void)
{

    rt_thread_t factorytest_handle_thread = rt_thread_create("factorytest_handle", factorytest_handle_task, RT_NULL, 512, 10, 10);
    if (factorytest_handle_thread != RT_NULL)
    {
        rt_thread_startup(factorytest_handle_thread);
    }
    else
    {
        LOG_E("factorytest_handle_thread create failed.");
    }
}
// ZINO_APP_EXPORT(factorytest_handle_task_entry);
